﻿#include "include.h"
#include "locomotion_header.h"
#include "math.h"
#include "eso.h"
#include "gait_math.h"
#include "i_5pos.h"
#include "f_5pos.h"
#if !RUN_WEBOTS
#include "arm_math.h"
#endif
//------------------------------运动学运算库
float FLT_SPD_END = 500;

void espd_to_neg_dq(int id, float dt)
{
	float flt_espd = 1;
	float gain_espd = 1;
	float temp[3] = { 0,0,0 };

	temp[0] = (-robotwb.Leg[id].jacobi_inv[0] * robotwb.Leg[id].tar_espd_h.x*vmc[id].param.invert_knee_epos[Xr]
		- robotwb.Leg[id].jacobi_inv[1] * robotwb.Leg[id].tar_espd_h.y//*robotwb.Leg[id].flag_rl
		+ (-robotwb.Leg[id].jacobi_inv[2] * robotwb.Leg[id].tar_espd_h.z))*gain_espd;
	temp[1] = (-robotwb.Leg[id].jacobi_inv[3] * robotwb.Leg[id].tar_espd_h.x*vmc[id].param.invert_knee_epos[Xr]
		- robotwb.Leg[id].jacobi_inv[4] * robotwb.Leg[id].tar_espd_h.y//*robotwb.Leg[id].flag_rl
		+ (-robotwb.Leg[id].jacobi_inv[5] * robotwb.Leg[id].tar_espd_h.z))*gain_espd;
	temp[2] = (-robotwb.Leg[id].jacobi_inv[6] * robotwb.Leg[id].tar_espd_h.x*vmc[id].param.invert_knee_epos[Xr]
		- robotwb.Leg[id].jacobi_inv[7] * robotwb.Leg[id].tar_espd_h.y//*robotwb.Leg[id].flag_rl
		+ (-robotwb.Leg[id].jacobi_inv[8] * robotwb.Leg[id].tar_espd_h.z))*gain_espd;

	robotwb.Leg[id].tar_sita_d[0] = temp[0] * flt_espd + (1 - flt_espd)*robotwb.Leg[id].tar_sita_d[0];
	robotwb.Leg[id].tar_sita_d[1] = temp[1] * flt_espd + (1 - flt_espd)*robotwb.Leg[id].tar_sita_d[1];
	robotwb.Leg[id].tar_sita_d[2] = temp[2] * flt_espd + (1 - flt_espd)*robotwb.Leg[id].tar_sita_d[2];
}

void force_to_tao(int id, float dt)
{
	float flt_t = 1;//滤波系数
	float gain_t = -1;//输出符号
	float temp[3] = { 0,0,0 };
	temp[0] = (robotwb.Leg[id].jacobi[0] * robotwb.Leg[id].tar_force_h.x*vmc[id].param.invert_knee_epos[Xr]
		+ (robotwb.Leg[id].jacobi[3] * robotwb.Leg[id].tar_force_h.y)
		+ (robotwb.Leg[id].jacobi[6] * robotwb.Leg[id].tar_force_h.z)
		)*gain_t;
	temp[1] = (robotwb.Leg[id].jacobi[1] * robotwb.Leg[id].tar_force_h.x*vmc[id].param.invert_knee_epos[Xr]
		+ (robotwb.Leg[id].jacobi[4] * robotwb.Leg[id].tar_force_h.y)
		+ (robotwb.Leg[id].jacobi[7] * robotwb.Leg[id].tar_force_h.z)
		)*gain_t;
	temp[2] = (robotwb.Leg[id].jacobi[2] * robotwb.Leg[id].tar_force_h.x*vmc[id].param.invert_knee_epos[Xr]
		+ (robotwb.Leg[id].jacobi[5] * robotwb.Leg[id].tar_force_h.y)
		+ (robotwb.Leg[id].jacobi[8] * robotwb.Leg[id].tar_force_h.z)
		)*gain_t;
	robotwb.Leg[id].taod[0] = temp[0] * flt_t + (1 - flt_t)*robotwb.Leg[id].taod[0];
	robotwb.Leg[id].taod[1] = temp[1] * flt_t + (1 - flt_t)*robotwb.Leg[id].taod[1];
	robotwb.Leg[id].taod[2] = temp[2] * flt_t + (1 - flt_t)*robotwb.Leg[id].taod[2];

	robotwb.Leg[id].taod[0] = limitw(robotwb.Leg[id].taod[0], -robotwb.Leg[id].limit_tao[0], robotwb.Leg[id].limit_tao[0]);
	robotwb.Leg[id].taod[1] = limitw(robotwb.Leg[id].taod[1], -robotwb.Leg[id].limit_tao[1], robotwb.Leg[id].limit_tao[1]);
	robotwb.Leg[id].taod[2] = limitw(robotwb.Leg[id].taod[2], -robotwb.Leg[id].limit_tao[2], robotwb.Leg[id].limit_tao[2]);
}

void force_to_tao_input(int id, Vect3 force_h, float dt)
{
	float flt_t = 1;
	float gain_t = -1;
	float temp[3] = { 0,0,0 };
	temp[0] = (robotwb.Leg[id].jacobi[0] * force_h.x*vmc[id].param.invert_knee_epos[Xr]
		+ (robotwb.Leg[id].jacobi[3] * force_h.y)
		+ (robotwb.Leg[id].jacobi[6] * force_h.z)
		)*gain_t;
	temp[1] = (robotwb.Leg[id].jacobi[1] * force_h.x*vmc[id].param.invert_knee_epos[Xr]
		+ (robotwb.Leg[id].jacobi[4] * force_h.y)
		+ (robotwb.Leg[id].jacobi[7] * force_h.z)
		)*gain_t;
	temp[2] = (robotwb.Leg[id].jacobi[2] * force_h.x*vmc[id].param.invert_knee_epos[Xr]
		+ (robotwb.Leg[id].jacobi[5] * force_h.y)
		+ (robotwb.Leg[id].jacobi[8] * force_h.z)
		)*gain_t;
	robotwb.Leg[id].taod[0] = temp[0] * flt_t + (1 - flt_t)*robotwb.Leg[id].taod[0];
	robotwb.Leg[id].taod[1] = temp[1] * flt_t + (1 - flt_t)*robotwb.Leg[id].taod[1];
	robotwb.Leg[id].taod[2] = temp[2] * flt_t + (1 - flt_t)*robotwb.Leg[id].taod[2];

	robotwb.Leg[id].taod[0] = limitw(robotwb.Leg[id].taod[0], -robotwb.Leg[id].limit_tao[0], robotwb.Leg[id].limit_tao[0]);
	robotwb.Leg[id].taod[1] = limitw(robotwb.Leg[id].taod[1], -robotwb.Leg[id].limit_tao[1], robotwb.Leg[id].limit_tao[1]);
	robotwb.Leg[id].taod[2] = limitw(robotwb.Leg[id].taod[2], -robotwb.Leg[id].limit_tao[2], robotwb.Leg[id].limit_tao[2]);
}



//正运动学
float k_end_v = 1;
char estimate_end_state_new(VMC *in, float dt)//??????
{
	float a = in->l3;
	float L1 = in->l1;
	float L2 = in->l2;
	float L4 = in->l4;

	float s0 = in->flag_rl*in->sita3;//侧展
	float s1 = in->sita1;
	float s2 = 180 - in->sita2;
	float H = L1 * cosd(s1) + L2 * cosd(-s1 + s2) + L4;
	float x = -(L1 * sind(-s1) + L2 * sind(-s1 + s2));
	float y = -in->flag_rl*(a * cosd(s0) + H * sind(s0));
	float z = -H * cosd(s0) + a * sind(s0);
	x *= in->param.invert_knee_epos[Xr];
	x *= -1;
	y *= -1;

	END_POS temp, tempc;
	temp.x = x;
	temp.y = y;
	temp.z = z;

	tempc.x = x;
	tempc.y = y;
	tempc.z = z - Hw;
#if EN_ROT_Q4//转换航向关节
	rotate_vect3_with_yaw(temp,    &in->epos,  in->sita4);
	rotate_vect3_with_yaw(tempc,   &in->eposc, in->sita4);
#else
	in->epos = temp;
	in->eposc = tempc;
#endif
	in->epos_b.x = in->epos.x;
	in->epos_b.y = in->epos.y + in->flag_rl* vmc_all.W / 2;
	in->epos_b.z = in->epos.z - Hw;//足端为-

	//膝关节位置
	x = -sind(s1)*L1;
	z = -cosd(s1)*L1;
	float sita_leg_knee_b = 180-atan2f(z-in->epos.z,  x-in->epos.x)*57.3;
	float sita_foot_b = in->sita5 - 90+ sita_leg_knee_b;
	float foot_q_off = 10;
	//足端角度
	in->sita_leg_knee_b = sita_leg_knee_b;
	in->sita_foot_b = sita_foot_b- foot_q_off;

	in->sita_leg_knee_n = sita_leg_knee_b-vmc_all.att[PITr];
	in->sita_foot_n = sita_foot_b - foot_q_off -vmc_all.att[PITr];
#if 0
		if (in->param.id == 0) {
			printf("xknee[%d] q=%f x=%.2f %.2f sita_leg_knee_b=%f sita_foot_b=%f\n", in->param.id, s1, x,z, sita_leg_knee_b, in->sita_foot_b);
			printf("sita_leg_knee_n=%f sita_foot_n=%f\n", in->sita_leg_knee_n, in->sita_foot_n);
			printf("hip[%d]   x=%.2f %.2f sita5=%f\n", in->param.id, in->epos.x, in->epos.z, in->sita5);
		}
#endif
	END_POS cog_b;
	cog_b.x = 0;
	cog_b.y = 0;	
	cog_b.z = Hw1;

	vmc_all.cog_n.x = vmc_all.Rb_n[0][0] * cog_b.x + vmc_all.Rb_n[0][1] * cog_b.y + vmc_all.Rb_n[0][2] * cog_b.z;
	vmc_all.cog_n.y = vmc_all.Rb_n[1][0] * cog_b.x + vmc_all.Rb_n[1][1] * cog_b.y + vmc_all.Rb_n[1][2] * cog_b.z;
	vmc_all.cog_n.z = vmc_all.Rb_n[2][0] * cog_b.x + vmc_all.Rb_n[2][1] * cog_b.y + vmc_all.Rb_n[2][2] * cog_b.z;

	in->epos_n.x = vmc_all.Rb_n[0][0] * in->epos_b.x + vmc_all.Rb_n[0][1] * in->epos_b.y + vmc_all.Rb_n[0][2] * in->epos_b.z;
	in->epos_n.y = vmc_all.Rb_n[1][0] * in->epos_b.x + vmc_all.Rb_n[1][1] * in->epos_b.y + vmc_all.Rb_n[1][2] * in->epos_b.z;
	in->epos_n.z = vmc_all.Rb_n[2][0] * in->epos_b.x + vmc_all.Rb_n[2][1] * in->epos_b.y + vmc_all.Rb_n[2][2] * in->epos_b.z;

	in->epos_n_b.x = vmc_all.Rb_n[0][0] * in->epos.x + vmc_all.Rb_n[0][1] * in->epos.y + vmc_all.Rb_n[0][2] * in->epos.z;
	in->epos_n_b.y = vmc_all.Rb_n[1][0] * in->epos.x + vmc_all.Rb_n[1][1] * in->epos.y + vmc_all.Rb_n[1][2] * in->epos.z;
	in->epos_n_b.z = vmc_all.Rb_n[2][0] * in->epos.x + vmc_all.Rb_n[2][1] * in->epos.y + vmc_all.Rb_n[2][2] * in->epos.z;
#if 0
	if (in->param.id == 0) {
		printf("q    id=%d q0=%.2f q1=%.2f q2=%.2f flagx=%d\n", in->param.id, in->sita1, in->sita2, in->sita3, in->param.invert_knee_epos[Xr]);
		printf("hip  id=%d hx=%.2f hy=%.2f hz=%.2f flagx=%d\n", in->param.id, in->epos.x, in->epos.y, in->epos.z, in->param.invert_knee_epos[Xr]);
		printf("body id=%d bx=%.2f by=%.2f bz=%.2f\n", in->param.id, in->epos_b.x, in->epos_b.y, in->epos_b.z);
		printf("n    id=%d nx=%.2f ny=%.2f nz=%.2f\n", in->param.id, in->epos_n.x, in->epos_n.y, in->epos_n.z);
	}
#endif
	float d_sita = 0;
	float d_alfa = 0;
	float d_sita1, d_sita2, d_sita3;
	float d_x, d_y, d_z;
	float dr_dt;
	in->param.spd_est_cnt += dt;
	//--------------------------末端速度-------------------------
	Vect3 spd_bb;
	Vect3 q_spd;

	float jacobi3[3][3] = { 0 };
	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			jacobi3[i][j] = in->jacobi33[i * 3 + j];
	q_spd.x = in->dsita1 / 57.3;
	q_spd.y = in->dsita2 / 57.3;
	q_spd.z = in->dsita3 / 57.3;//角速度有值 会后退在支撑
	matrx33_mult_vect3(jacobi3, q_spd, &spd_bb);//doghome
	spd_bb.x *= in->param.invert_knee_epos[Xr];

#if EN_ROT_Q4//转换航向关节
	Vect3 tempv = spd_bb;
	rotate_vect3v_with_yaw(tempv, &spd_bb, in->sita4);
#endif
#if !RUN_PI&&0
	if (in->param.spd_est_cnt > vmc_all.param.end_sample_dt) {
		d_x = (in->epos.x - in->epos_reg.x) / in->param.spd_est_cnt;
		d_y = (in->epos.y - in->epos_reg.y) / in->param.spd_est_cnt;
		d_z = (in->epos.z - in->epos_reg.z) / in->param.spd_est_cnt;

		if (in->ground || 1) {
			in->spd_o.x = -LIMIT(d_x, -5, 5);
			in->spd_o.y = -LIMIT(d_y, -5, 5);
			in->spd_o.z = LIMIT(d_z, -5, 5);
		}
		in->epos_reg.x = in->epos.x;
		in->epos_reg.y = in->epos.y;
		in->epos_reg.z = in->epos.z;
		in->param.spd_est_cnt = 0;
	}
#else
	in->spd_o.x = -LIMIT(spd_bb.x, -2, 2);
	in->spd_o.y = -LIMIT(spd_bb.y, -2, 2);
	in->spd_o.z = LIMIT(spd_bb.z,  -2, 2);
#endif
	DigitalLPF(in->spd_o.x, &in->spd.x, FLT_SPD_END, dt);
	DigitalLPF(in->spd_o.y, &in->spd.y, FLT_SPD_END, dt);
	DigitalLPF(in->spd_o.z, &in->spd.z, FLT_SPD_END, dt);
	//printf("body id=%d vx=%f vy=%f vz=%f\n", in->param.id, in->spd.x, in->spd.y, in->spd.z);
	//printf("body id=%d bx=%.3f by=%.3f bz=%.3f\n", in->param.id, in->epos_b.x, in->epos_b.y, in->epos_b.z);
	in->spd_b.x = in->spd.x;
	in->spd_b.y = in->spd.y;
	in->spd_b.z = in->spd.z;

	END_POS spd_n = in->spd_o;
	spd_n.x = vmc_all.Rb_n[0][0] * in->spd_o.x + vmc_all.Rb_n[0][1] * in->spd_o.y + vmc_all.Rb_n[0][2] * in->spd_o.z;
	spd_n.y = vmc_all.Rb_n[1][0] * in->spd_o.x + vmc_all.Rb_n[1][1] * in->spd_o.y + vmc_all.Rb_n[1][2] * in->spd_o.z;
	spd_n.z = vmc_all.Rb_n[2][0] * in->spd_o.x + vmc_all.Rb_n[2][1] * in->spd_o.y + vmc_all.Rb_n[2][2] * in->spd_o.z;

#if EN_ROTATE_END_COMPASS_EST&&1//旋转速度补偿
	Vect3 Hip_b;
	Vect3 com;
	com.x = com.y = com.z = 0;
	Hip_b.x = com.x - in->epos_b.x;
	Hip_b.y = com.y - in->epos_b.y;
	Hip_b.z = com.z - in->epos_b.z;

	Vect3 w_b;
	w_b.x = vmc_all.att_rate_trig[ROLr] * 0.0173 * (vmc_all.param.leg_dof == 3);
	w_b.y = vmc_all.att_rate_trig[PITr] * 0.0173 * 1;
	w_b.z = vmc_all.att_rate_trig[YAWr] * 0.0173 * 0;
	float w_cross_b[3][3] = { 0 };
	if (1) {//in->ground || 1) {
		vect3_2_cross(w_b, w_cross_b);
		Vect3 hip_rotate_spd_b, hip_rotate_spd_n;

		matrx33_mult_vect3(w_cross_b, Hip_b, &hip_rotate_spd_b);
		converV_b_to_n(hip_rotate_spd_b.x, hip_rotate_spd_b.y, hip_rotate_spd_b.z,
			&hip_rotate_spd_n.x, &hip_rotate_spd_n.y, &hip_rotate_spd_n.z);//转换速度直接输出到IMP  控制频率不够目前？？
		if (vmc_all.gait_mode == CLIMB
			|| vmc_all.gait_mode == BOUND
			|| vmc_all.gait_mode == WALK
			|| vmc_all.gait_mode == STAND_RC
			) {
			spd_n.x -= hip_rotate_spd_n.x;
			spd_n.y -= hip_rotate_spd_n.y;
			spd_n.z -= hip_rotate_spd_n.z;
		}
	}
#endif
	DigitalLPF(spd_n.x, &in->spd_n.x, FLT_SPD_END, dt);
	DigitalLPF(spd_n.y, &in->spd_n.y, FLT_SPD_END, dt);
	DigitalLPF(spd_n.z, &in->spd_n.z, FLT_SPD_END, dt);

	//胯关节速度
	Vect3 v_hip_R;
	v_hip_R.x = 0;
	v_hip_R.y = vmc_all.W / 2 * in->flag_rl;
	v_hip_R.z = 0;
	Vect3 p_hip_R;
	converV_b_to_nw(v_hip_R, &p_hip_R);
	Vect3 ws;
	Vect3 w_bb;
	float w_cross_bs[3][3] = { 0 };

	w_bb.x = vmc_all.att_rate_swing[ROLr] * 0.0173 * 1;
	w_bb.y = vmc_all.att_rate_swing[PITr] * 0.0173 * 1;//PIT -1 Trot good   1 bound good
	w_bb.z = vmc_all.att_rate_swing[YAWr] * 0.0173 * 1;

	converV_b_to_nw(w_bb, &ws);
	vect3_2_cross(ws, w_cross_bs);//转换成 反对称矩阵

	Vect3 hip_rotate_spd_bs;
	matrx33_mult_vect3(w_cross_bs, p_hip_R, &hip_rotate_spd_bs);
	in->spd_hip_n.x = vmc_all.spd_n.x + hip_rotate_spd_bs.x;
	in->spd_hip_n.y = vmc_all.spd_n.y + hip_rotate_spd_bs.y;
	in->spd_hip_n.z = vmc_all.spd_n.z + hip_rotate_spd_bs.z;
	if (in->param.id == 0 && 0)
		printf("%f %f %f\n", hip_rotate_spd_bs.x, hip_rotate_spd_bs.y, hip_rotate_spd_bs.z);

	return 0;
}

void inv_KI(int id, Vect3 epos_h, float *s0, float *s1, float *s2)
{
	float end_pos[3], end_posf[3];
	float temp[6];

	end_pos[Xrw] = vmc[id].param.invert_knee_epos[Xr] * epos_h.x;
	end_pos[Yrw] = robotwb.Leg[id].flag_rl* epos_h.y;
	end_pos[Zrw] = epos_h.z - Hw;

	float Rr1 = end_pos[Yrw] * end_pos[Yrw] + end_pos[Zrw] * end_pos[Zrw];
 
	temp[2] = sqrtf(limitw(-L3w * L3w + Rr1, 0, 99));
	*s2 = (90 - atan2(fabs(end_pos[Zrw]), end_pos[Yrw] + 0.000001)*RAD_TO_DEGw -
		atan2(L3w, temp[2] + 0.000001)*RAD_TO_DEGw)*robotwb.Leg[id].flag_rl;//侧展

	temp[0] = sqrtf(limitw(Rr1 - L3w * L3w, 0, 99))-L4w;//h1
	temp[1] = sqrtf(limitw(temp[0] * temp[0] + end_pos[Xrw] * end_pos[Xrw], 0, 99));//r 平面足端连线长度

	temp[2] = atan2(end_pos[Xrw], temp[0] + 0.000001)*RAD_TO_DEGw;//sita
	temp[3] = (temp[1] * temp[1] + L1w * L1w - L2w * L2w) / (2 * L1w*temp[1] + 0.000001);
	temp[3] = limitw(temp[3], -1, 1);
	*s0 = 180 - (180 + temp[2] - acos(temp[3]) *RAD_TO_DEGw);//大腿

	temp[3] = (-temp[1] * temp[1] + L1w * L1w + L2w * L2w) / (2 * L1w*L2w + 0.000001);
	temp[3] = limitw(temp[3], -1, 1);
	*s1 = acos(temp[3])*RAD_TO_DEGw;//小腿
	//float Rr = end_pos[Xrw] * end_pos[Xrw] + end_pos[Yrw] * end_pos[Yrw] + (end_pos[Zrw] + L4w) * (end_pos[Zrw] +L4w);
	//temp[0] = (L1w*L1w + L2w * L2w + L3w * L3w - Rr) / (2 * L1w*L2w);
	//temp[0] = limitw(temp[0], -1, 1);
	//*s1 = acos(temp[0])*RAD_TO_DEGw;//小腿
}



void inv_KI_hip(int id, Vect3 epos_h, float *s0, float *s1, float *s2)
{
	float end_pos[3], end_posf[3];
	float temp[6];

	end_pos[Xrw] = vmc[id].param.invert_knee_epos[Xr] * epos_h.x;
	end_pos[Yrw] = robotwb.Leg[id].flag_rl* epos_h.y;
	end_pos[Zrw] = epos_h.z - Hw*0;

	float Rr1 = end_pos[Yrw] * end_pos[Yrw] + end_pos[Zrw] * end_pos[Zrw];

	temp[2] = sqrtf(limitw(-L3w * L3w + Rr1, 0, 99));
	*s2 = (90 - atan2(fabs(end_pos[Zrw]), end_pos[Yrw] + 0.000001)*RAD_TO_DEGw -
		atan2(L3w, temp[2] + 0.000001)*RAD_TO_DEGw)*robotwb.Leg[id].flag_rl;//侧展

	temp[0] = sqrtf(limitw(Rr1 - L3w * L3w, 0, 99)) - L4w;//h1
	temp[1] = sqrtf(limitw(temp[0] * temp[0] + end_pos[Xrw] * end_pos[Xrw], 0, 99));//r 平面足端连线长度

	temp[2] = atan2(end_pos[Xrw], temp[0] + 0.000001)*RAD_TO_DEGw;//sita
	temp[3] = (temp[1] * temp[1] + L1w * L1w - L2w * L2w) / (2 * L1w*temp[1] + 0.000001);
	temp[3] = limitw(temp[3], -1, 1);
	*s0 = 180 - (180 + temp[2] - acos(temp[3]) *RAD_TO_DEGw);//大腿

	temp[3] = (-temp[1] * temp[1] + L1w * L1w + L2w * L2w) / (2 * L1w*L2w + 0.000001);
	temp[3] = limitw(temp[3], -1, 1);
	*s1 = acos(temp[3])*RAD_TO_DEGw;//小腿
	//float Rr = end_pos[Xrw] * end_pos[Xrw] + end_pos[Yrw] * end_pos[Yrw] + (end_pos[Zrw] + L4w) * (end_pos[Zrw] +L4w);
	//temp[0] = (L1w*L1w + L2w * L2w + L3w * L3w - Rr) / (2 * L1w*L2w);
	//temp[0] = limitw(temp[0], -1, 1);
	//*s1 = acos(temp[0])*RAD_TO_DEGw;//小腿
}

char inv_end_state_new(VMC *vmc, float x, float y, float z, float *sita1, float *sita2, float *sita3)
{
	int i = vmc->param.id;
	robotwb.Leg[i].tar_epos_h.x = x;
	robotwb.Leg[i].tar_epos_h.y = y;
	robotwb.Leg[i].tar_epos_h.z = z;
	inv_KI(i, robotwb.Leg[i].tar_epos_h, sita1, sita2, sita3);
	return 1;
}

char cal_invjacobi(VMC *in)
{
	float det = 0;
	char i = 0;
	invet33(in->jacobi33, &det, in->ijacobi33);
	if (fabs(det) < 0.00001)
		return 0;
	return 1;
}

char cal_jacobi_new(VMC *in)
{
	float Jcobi[9];
#if 1
	Jcobi[0] =//in->param.invert_knee_epos[Xr]*
		-in->l1 * cosdw(in->sita1) - in->l2 * cosdw(180 - in->sita2 - in->sita1);//x p_D
	Jcobi[1] =//in->param.invert_knee_epos[Xr]*
		-in->l2 * cosdw(180 - in->sita2 - in->sita1);//x p_X
	Jcobi[2] =//in->param.invert_knee_epos[Xr]*
		0;//x p_T
	float h = in->l1 * cosdw(in->sita1) + in->l2 * cosdw(180 - in->sita2 - in->sita1)+in->l4;
	float h_s1 = -in->l1 * sindw(in->sita1) + in->l2 * sindw(180 - in->sita2 - in->sita1);
	float h_s2 = in->l2 * sindw(180 - in->sita2 - in->sita1);
	float h_s3 = 0;

	Jcobi[3] = in->flag_rl*//in->param.invert_knee_epos[Yr]*
		h_s1 * sindw(in->sita3 * in->flag_rl);//y p_D
	Jcobi[4] = in->flag_rl*//in->param.invert_knee_epos[Yr]*
		h_s2 * sindw(in->sita3 * in->flag_rl);//y p_X
	Jcobi[5] = in->flag_rl*//in->param.invert_knee_epos[Yr]*
		(-in->l3 * sindw(in->sita3 * in->flag_rl)*in->flag_rl +
			h_s3 * sindw(in->sita3 * in->flag_rl) +
			h * cosdw(in->sita3 * in->flag_rl)*in->flag_rl);//y p_T

	Jcobi[6] = -h_s1 * cosdw(in->sita3 * in->flag_rl);//z p_D
	Jcobi[7] = -h_s2 * cosdw(in->sita3 * in->flag_rl);//z p_X
	Jcobi[8] = -h_s3 * cosdw(in->sita3 * in->flag_rl)
		+ h * sindw(in->sita3 * in->flag_rl)*in->flag_rl
		+ in->l3 * cosdw(in->sita3)*in->flag_rl;//z p_T
#endif
	for (int i = 0; i < 9; i++)
		in->jacobi33[i] = Jcobi[i];
 
	return 1;
}
